#include "gpt_robot_description/diffbot_hardware_interface.h"

DiffBotHWInterface::DiffBotHWInterface(ros::NodeHandle& nh) : nh_(nh)
{
}

void DiffBotHWInterface::init()
{
    // Initialize hardware interface
    hardware_interface::JointStateHandle state_handle_left("left_wheel_joint", &pos_[0], &vel_[0], &eff_[0]);
    jnt_state_interface_.registerHandle(state_handle_left);

    hardware_interface::JointStateHandle state_handle_right("right_wheel_joint", &pos_[1], &vel_[1], &eff_[1]);
    jnt_state_interface_.registerHandle(state_handle_right);

    hardware_interface::JointHandle vel_handle_left(jnt_state_interface_.getHandle("left_wheel_joint"), &cmd_[0]);
    jnt_vel_interface_.registerHandle(vel_handle_left);

    hardware_interface::JointHandle vel_handle_right(jnt_state_interface_.getHandle("right_wheel_joint"), &cmd_[1]);
    jnt_vel_interface_.registerHandle(vel_handle_right);

    registerInterface(&jnt_state_interface_);
    registerInterface(&jnt_vel_interface_);
}

void DiffBotHWInterface::read(const ros::Duration& period)
{
    // Read the state of the hardware (e.g., from sensors)
    // Replace with actual sensor read code
    pos_[0] += vel_[0] * period.toSec();
    pos_[1] += vel_[1] * period.toSec();
    ROS_INFO("Read - Left wheel position: %f, velocity: %f", pos_[0], vel_[0]);
    ROS_INFO("Read - Right wheel position: %f, velocity: %f", pos_[1], vel_[1]);
}

void DiffBotHWInterface::write(const ros::Duration& period)
{
    // Send the command to the hardware (e.g., to actuators)
    // Replace with actual motor control code
    double left_wheel_velocity = cmd_[0];
    double right_wheel_velocity = cmd_[1];
    ROS_INFO("Write - Left wheel command: %f, Right wheel command: %f", left_wheel_velocity, right_wheel_velocity);
    // Example:
    // motor_controller_left.setVelocity(left_wheel_velocity);
    // motor_controller_right.setVelocity(right_wheel_velocity);
}
